#include "windmill.hpp"
#include"track.hpp"
#include <chrono>
#include<opencv2/imgcodecs.hpp>
#include<opencv2/highgui.hpp>
#include<opencv2/imgproc.hpp>
#include<iostream>
#include<cmath>
#include<stdio.h>
#include<ceres/ceres.h>

double time_data[200]={0};
double speed_data[200];

double get_angle(float x1,float y1,float x2,float y2)                 //坐标转化为角
{
    double angle=acos((x1*x2+y1*y2)/sqrt((x1*x1+y1*y1)*(x2*x2+y2*y2)));
	  return angle;

}
void get_data()                                                        //得到速度与时间数据；
{
    cv::Point2f R_temp;
	cv::Point2f tar_temp;
	double  start=std::chrono::system_clock::now().time_since_epoch().count();
  double t=0;                                                           
	int i=0;
	int flag=1;
	Track wind;



	WINDMILL::WindMill
		wm((std::chrono::system_clock::now().time_since_epoch().count()-start )/ 1e9);
	do
	{
		cv::Mat imge =
			wm.getMat((std::chrono::system_clock::now().time_since_epoch().count() -start)/ 1e9);

		wind.find_contour(imge);
		
    

		t=(std::chrono::system_clock::now().time_since_epoch().count()-start)/1e6;
	
		
		if((t-time_data[i])>20)                                                                   //每二十多毫秒记录一次
		{

			time_data[i+1]=t;
			speed_data[i+1]=1000*get_angle(wind.target.x-wind.R.x,wind.target.y-wind.R.y,
			tar_temp.x-R_temp.x,tar_temp.y-R_temp.y)/(t-time_data[i]);
			//std::cout<<time_data[i+1];
			//std::cout<<","<<speed_data[i+1]<<std::endl;
			flag=1;
			i++;
		}    
        if(i==200) break;                //共取200个数据


		if(flag==1)               //记录该点的数据，便于与下一个点做差。
		{
			R_temp.x=wind.R.x;
		    R_temp.y=wind.R.y;
		    tar_temp.x=wind.target.x;
		    tar_temp.y=wind.target.y;
			flag=0;
		}


	} while (cv::waitKey(1) != 'q');
	
};

using namespace std;


//  int main()                     //获得a随时间变化的数据，用了暴力求解的方式 
//  {
//    get_data();
//    float a,w;
//    int position;
//    for(int j=1;j<=200;j++)
//    {
//        for(double i=1.85;i>=1.7;i=i-0.0000001)
//        {
//          if(abs(0.785*sin(1.884*time_data[j]/1000+i)+1.305-speed_data[j])<=0.01)
//          {
//            //cout<<time_data[j]/1000<<endl;
//            cout<<i<<endl;
//            break;
//          }
//        }
//    }
  
//  }


	//构建代价函数结构体，abc为待优化参数，residual为残差。
struct CURVE_FITTING_COST
{
  CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
  template <typename T>
  bool operator()(const T* const abc,T* residual)const
  {
    residual[0]=(T(_y)-0.785*(ceres::sin(1.884*T(_x)+abc[0]))-1.305);
    return true;
  }
  const double _x,_y;
};

using namespace std;
//主函数
int main()
{

  get_data();
  //参数初始化设置
  double abc[1] = {0};

//生成待拟合曲线的数据散点，储存在Vector里，x_data，y_data。
  vector<double> x_data,y_data;
  for(int i=0;i<100;i++)
  {
    double x=time_data[i+1]/1000;
	  double y=speed_data[i+1];
    x_data.push_back(x);
	
    y_data.push_back(y);
  
  }
//将每个点的残差累计求和构建最小二乘优化式
//不使用核函数，待优化参数是abc
  ceres::Problem problem;
  for(int i=0;i<100;i++)
  {
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,1>(
        new CURVE_FITTING_COST(x_data[i],y_data[i])
      ),
      nullptr,
      abc
    );
  }

//配置求解器并求解，输出结果
  ceres::Solver::Options options;
  options.linear_solver_type=ceres::DENSE_QR;
  options.minimizer_progress_to_stdout=true;
  ceres::Solver::Summary summary;
  ceres::Solve(options,&problem,&summary);
  cout<<"a= "<<abc[0]<<endl;
return 0;
};


